#ifndef RSIM_JOINT_H
#define RSIM_JOINT_H

/* -------------------------------------------------------------------------- *
 * File: Joint.h                                                              *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Stage.h"
#include "State.h"
#include "Transform.h"
#include "JointType.h"
#include "JointUtils.h"
#include "FastOps.h"
        
namespace RSIM{

// TODO: Figure out how to include custom joints following the procedure described in 
// "Minimal formulation of joint motion for biomechanisms", Seth, Delp, Nonlinear Dynamics,
// 2010.

// kdb: 9th oct 2010: ^^ should be doable. One just need to be careful when passing over the
// custom joint. No reason for rest of the code to change.

class UIndex;
class QIndex;
class ABAData;
class EOMData;
class RigidBodyNode;
class ExternalSpatialForce;
class SystemAcceleration;
class PrescribedMotion;

/**	
This is the base class for all kinematic joints.
*/
class Joint:public JointUtils{
        private:
                void operator=(const Joint& J);
        protected:
                
                /** Index of node associated wiht this joint. This can be used to extract
                Q,U,QDot and UDot from the state cache. */
                int NodeIndex_;
                
                /** Rotation matrix giving the orientation of frame F of the body
                with respect to the frame M of the parent. Default rotation
                is identity matrix */
                Transform T_MF_;
                
                /** A user defined motion as a derived class of PrescribedMotion. */
                PrescribedMotion *PM_;
                
                bool isMotionPrescribedAndActive_;
                
                /** Deep copy */
                Joint(const Joint& J);
                
        public:		
                Joint(const JointType& JT);				
                
                /** Clear joint cache */
                ~Joint();				

                /** Add a prescribed motion to this joint. For the moment, the prescribed motion
                is on all the mobilities of the joint. */
                void addPrescribedMotion(PrescribedMotion* Ptr);
                
                PrescribedMotion* getPrescribedMotion() const;
                
                /** @return Transform T_MF */
                const Transform& getTransformMF() const;
                
                /** @return Rotation R_MF */
                const Rotation& getRotationMF() const;
                
                /** get number of generalized coordinates associated with this joint */
                const int getNQ(const State& state) const;
                
                /** get number of generalized coordinates asscociatd with this joint */
                const int getNU(const State& state) const;

                const double* getQ(const State& state) const;
                
                const double* getQDot(const State& state) const;
                
                const double* getU(const State& state) const;
                
                const double* getUDot(const State& state) const;
                
                const double getOneU(const State& state, const UIndex&) const;
                
                const double getOneQ(const State& state, const QIndex&) const;
                
                double* wgetQ(State& state );
                
                double* wgetQDot(State& state);
                
                double* wgetU(State& state);
                
                double* wgetUDot(State& state);
                
                const bool getIsMotionPrescribedAndActive()const;
                
                void stopPrescribedMotion();
                
                void continuePrescribedMotion();
                
                void updOneQ(State& state, const QIndex& qindex, const double& q);
                
                void updOneU(State& state, const UIndex& uindex, const double& u);
                
                void setQFromPrescribedMotion(State& state);
                
                void setUFromPrescribedMotion(State& state);
                
                ///////////////////////////////////////////////////////
                // Following are the virtual functions which must be 
                // written for every new joint. Note, nQ_ and nU_ must be
                // set in the constructor for each new joint manually in
                // addition to an impementation of the virtual functions.
                ///////////////////////////////////////////////////////                                        
                
                virtual int getDOF()const=0;
                                
                virtual void getHColumn(const int& uindex, 
                                        double *hptr)const=0;
                                        
                virtual void calcShiftPartialLinear(const Vect3& beta,
                                                    const int& uindex, 
                                                    const Rotation& R_NF,
                                                    double *dv)const{};
                virtual void setNQ(int *q)const=0;
                
                virtual void setNU(int *u)const=0;
                
                /** Provide the default values for each joint Q*/
                virtual void setDefaultQ(double *q)const = 0;
                                
                /** Provide the default values for each joint U*/
                virtual void setDefaultU(double *u)const = 0;
                
                virtual const bool areQuaternionsInUse()const=0;
                
                /** Return the degrees of freedom for this joint. */
                virtual int getJointDOF() const = 0;
                                        
                /** Update joint Transform and compute betak */
                virtual void updJointTransform(const double *q,
                                        const RigidBodyNode *Parent,
                                        const int& pChildID,
                                        double *betak) = 0;

                virtual void updBetaK(	const double *q,
                                        const RigidBodyNode *Parent,
                                        const int& pChildID,
                                        double *betak)const{};

                /** Calculate Qdot for this joint. This function is only required for 
                Ball-like joints. Such joints use Euler parameters and need this additional
                step to compute the QDots. */
                virtual void calcQDot(State& state)const=0;

                /** Print the joint type */
                virtual void printJointType() const = 0;

                // It is assumed that all elements of h_x_udot are initialzed to 0.0
                virtual void calc_H_x_UDot(const State& state, double *h_x_udot) const = 0;

                /** 
                @param w is the absolute angular velocity of the body expressed in F frame which is 
                updated with this function call.
                @param v is the absolute linear velocity of the body expressed in F frame which is 
                updated with this function call.
                */
                virtual void updVelocity(	const State& state,
                                                const RigidBodyNode *Parent,
                                                const Rotation& prF_C_F,
                                                const double *betak,
                                                Vect3& w,
                                                Vect3& v) const = 0;

                /** 
                @param w angular velocity of this body with respect to the parent expressed in F frame 
                @param v linear velocity of the origin of this body with respect to the parent expressed in F frame of this body
                */
                virtual void updRelVelocity(const State& state, 
                                        Vect3& w, 
                                        Vect3& v) const = 0;		
                
                /**
                Call to this function updates the QDot field from the state cache, 
                If motion is prescribed for this joint, the UDot cache is updated 
                with this function call.
                */
                virtual void calcAKT(const RigidBodyNode *Parent,
                                const RigidBodyNode *Body,
                                const int& ChildID,
                                const double *betak,
                                State& state,
                                Vect6& sAkt) const = 0;
                
                virtual void sweepForward(const RigidBodyNode *Parent,
                                        const int& ChildID,
                                        const Vect6& sAkt,
                                        const Rotation& FCF,
                                        const ABAData *aba,
                                        const double *betak,
                                        EOMData *eom,
                                        State& state
                                        ) = 0;
                
                virtual void sweepBackwards(const bool& isLeaf, 
                                        const int& ChildID,
                                        const Vect6& sAkt,
                                        const Rotation& FCF,					    
                                        const EOMData *eom,
                                        const double *betak,
                                        RigidBodyNode *Parent,
                                        ABAData *aba) const = 0;
                                        
};

inline const double* Joint::getQ(const State& state) const{
        return state.getQForNode(NodeIndex_);
}

inline const double* Joint::getQDot(const State& state) const{
        return state.getQDotForNode(NodeIndex_);
}

inline const double* Joint::getU(const State& state) const {
        return state.getUForNode(NodeIndex_);
}

inline const double* Joint::getUDot(const State& state) const{
        return state.getUDotForNode(NodeIndex_);
}

inline double* Joint::wgetUDot(State& state){
        return state.wgetUDotForNode(NodeIndex_);
}

inline double* Joint::wgetQ(State& state){
        return state.wgetQForNode(NodeIndex_);
}

inline double* Joint::wgetQDot(State& state){
        return state.wgetQDotForNode(NodeIndex_);
}

inline double* Joint::wgetU(State& state){
        return state.wgetUForNode(NodeIndex_);
}

inline const Transform& Joint::getTransformMF() const{
        return T_MF_;
}

inline PrescribedMotion* Joint::getPrescribedMotion() const{
        return PM_;
}

inline const bool Joint::getIsMotionPrescribedAndActive()const{
        return isMotionPrescribedAndActive_;
}

inline void Joint::addPrescribedMotion(PrescribedMotion *pm){
        this->isMotionPrescribedAndActive_ = true;
        this->PM_ = pm;
}

inline const Rotation& Joint::getRotationMF()const{
        return T_MF_.R();
}

inline const int Joint::getNQ(const State& state) const{
        return state.getBodyNQ(NodeIndex_);
}

inline const int Joint::getNU(const State& state) const{
        return state.getBodyNU(NodeIndex_);
}

} // namespace RSIM
#endif